import './TRobot.css';
import React, { Component } from "react";
import * as THREE from 'three';
import { Euler, LoadingManager } from 'three';
import { OrbitControls } from 'three/examples/jsm/controls/OrbitControls';
import Stats from 'three/examples/jsm/libs/stats.module';
import GUI from 'three/examples/jsm/libs/lil-gui.module.min';
import URDFLoader from 'urdf-loader';
import { getInverse, findNearest } from './InverseSolver';
import { getDTCP, getTCP } from './ForwardSolver';
import {  d2r, rpy2rv, rv2rpy } from './Common';

// const STEPS_PER_FRAME = 5;

export default
class TRobot extends Component{
  constructor(props) {
    super(props);

    this.baseUrl = props.BaseUrl || '.';
    this.indexUrl = props.IndexUrl || `${this.baseUrl}/data/robot.json`;

    this.state = {
      robotList:[],
      selectIndex:-1,
      robot:null,
      TCP:[0,0,0,0,0,0],

      showJointPanel:true,
      showPositionPanel:true,
      showServerPanel:true,

      serverIp:'192.168.0.125',
      serverPort:'30004',
      serverSync: true,
    }

    this.handleServerChanged = this.handleServerChanged.bind(this)

    this.handleIndexChanged = this.handleIndexChanged.bind(this);
    this.handleMouseDown = this.handleMouseDown.bind(this);
    this.handleMouseUp = this.handleMouseUp.bind(this);

    this.handleJointValueIncrease = this.handleJointValueIncrease.bind(this);
    this.handleJointValueDecrease = this.handleJointValueDecrease.bind(this);

    this.connect = this.connect.bind(this);
    this.disconnect = this.disconnect.bind(this);
    
    this.refContainer = React.createRef();
  }

  componentDidMount() {
    this.direction = {};
    this.jointControls = {};
    this.targetValues = [];
    this.clock = new THREE.Clock();
    this.loadRobotIndex( this.indexUrl );

    this.websock = undefined;

    window.addEventListener('resize', this.onWindowResize.bind(this));
    window.addEventListener('mouseup', this.handleMouseUp, false);
    window.addEventListener('touchend',this.handleMouseUp, false);
  }

  componentWillUnmount() {
    if( this.gui ){
      this.gui.destroy();
    }
  }

  onWindowResize(){
    const container = document.getElementById('container');
    
    if( !container ) return;
    
    if( this.camera ){
      this.camera.aspect = container.clientWidth / container.clientHeight;
      this.camera.updateProjectionMatrix();
    }

    if( this.renderer ) {
      this.renderer.setPixelRatio( container.devicePixelRatio );
      this.renderer.setSize( container.clientWidth, container.clientHeight );
    }
  }

  handleServerChanged( event ) {
    const id = event.target.id;

    switch( id ) {
      case 'ip':
        this.setState({ serverIp: event.target.value });
        break;
      case 'port':
        this.setState({ serverPort: event.target.value });
        break;
      case 'sync':
        this.setState({ serverSync: event.target.value });
        break;
      default:
        break;
    }
  }

  handleIndexChanged( event ) {
    const selected = event.target.id;
    if( !selected ) return;
    console.log( 'select robot:', selected );
    this.loadRobot( this.state.robotList[selected].robot );
    this.setState({ selectIndex : selected });
  }

  handleMouseDown(event) {
    this.direction[event.target.id] = true;
  }

  handleJointValueIncrease (event) {
    this.jointControls[event.target.id] = 1;
  }

  handleJointValueDecrease (event) {
    this.jointControls[event.target.id] = -1;
  }

  handleMouseUp() {
    this.direction={};
    this.jointControls = {};
  }

  initGraphics() {

    const container = this.refContainer.current; // document.getElementById('container');
    container.innerText = '';

    const renderer = new THREE.WebGLRenderer({ preserveDrawingBuffer: true });
    renderer.setPixelRatio( window.devicePixelRatio );
    renderer.setSize( container.clientWidth, container.clientHeight );

    container.appendChild( renderer.domElement );
    const scene = new THREE.Scene();
    scene.background = new THREE.Color(1, 1, 1);
    const camera = new THREE.PerspectiveCamera( 
      65, 
      container.clientWidth/container.clientHeight, 
      0.1,
      1000 );
    camera.position.set( 0, 1, 0 );

    const control = new OrbitControls( camera, renderer.domElement );
    control.target.set( 0, 0, 0 );

    const stats = new Stats();
    stats.domElement.style.position = 'absolute';
    stats.domElement.style.top = '0px';
    container.appendChild( stats.domElement );

    const ambientLight = new THREE.AmbientLight( 0xffffff, 1.5 );
    scene.add(ambientLight);

    const gridHelper = new THREE.GridHelper( 10, 100 );
    scene.add(gridHelper);

    const axesHelper = new THREE.AxesHelper();
    scene.add( axesHelper );

    this.renderer = renderer;
    this.scene = scene;
    this.camera = camera;
    this.control = control;
    this.stats = stats;
    this.ambientLight = ambientLight;


    this.initGUI();
    this.animate();

  }

  initGUI() {

    const gui = new GUI();

    const params = {
      'bgColor':0xffffff,
      'lightColor':0xffffff,
      'lightIntensity':5.0,

      'showJointPanel':true,
      'showPositionPanel':true,
      'showServerPanel':true,
      'av':1,
      'lv':0.2,
    }

    const eviroment = gui.addFolder('环境');

    eviroment.addColor(params, 'bgColor').name('背景色').onChange( (val)=>{
      this.scene.background = new THREE.Color( val );
    })

    eviroment.addColor(params, 'lightColor').name('环境光').onChange( (val)=>{
      this.ambientLight.color = new THREE.Color( val );
    })

    eviroment.add(params, 'lightIntensity', 0.1, 10).name('光强度').onChange( (val)=>{
      this.ambientLight.intensity = val;
    })

    eviroment.close();

    gui.add(params, 'showServerPanel').name('远程连接面板').onChange(val=>{
      this.setState({ showServerPanel: val });
    });
    gui.add(params, 'showPositionPanel').name('位置控制面板').onChange(val=>{
      console.log(val)
      this.setState({ showPositionPanel: val });
    });
    gui.add(params, 'showJointPanel').name('关节控制面板').onChange(val=>{
      this.setState({ showJointPanel: val });
    });

    gui.add(params, 'av', 0.1, Math.PI * 2).name('角速度');
    gui.add(params, 'lv', 0.1, 3).name('线速度');

    gui.close();

    this.gui = gui;
    this.params = params;

  }

  start() {

  }

  connect() {
    
    let ws = this.websock;
    const url = `ws://${this.state.serverIp}:${this.state.serverPort}`;
    console.log('connect to:', url);
    if( !ws ) {
      ws = new WebSocket(url);
      ws.onopen = ()=>{
        console.log('connected.')
      };

      ws.onmessage = (event)=>{
        console.log(event);
      };

      ws.onclose = ()=>{
        console.error('error');
      };
    }
    
  }

  disconnect() {
   
  }

  moveToTarget( target, velocity ) {
    const robot = this.robot;
    const joints = this.state.robotList[this.state.selectIndex].robot.joints;

    for( var i = 0; i < target.length; i ++ ) {
      const joint = robot.joints[joints[i]];

      var dir = (target[i] > joint.angle) ? 1:-1;

      const diff = Math.abs( target[i] - joint.angle );

      if( diff <= 0.00001 ){
        dir = 0;
      }
      else if( diff > 0.00001 && diff <= 0.001 ){
        dir *= 0.001;
      }
      else if( diff > 0.001 && diff <= 0.01 ){
        dir *= 0.01;
      }

      const angle = joint.angle + dir * velocity;
      joint.setJointValue(angle);
    }
  }

  getJointValues() {
    const robot = this.robot;
    const joints = this.state.robotList[this.state.selectIndex].robot.joints;
    const model = this.state.robotList[this.state.selectIndex].robot.model;
    const theta = [];
    if( !robot ) return;

    for( var i = 0, il = joints.length; i < il; i++ ) {
      theta[i] = robot.joints[joints[i]].angle;
    }

    return theta;
  }

  updateRobot( deltaTime ) {

    if( !this.robot ) return;

    // 位置控制
    const dAngle = deltaTime * this.params.av;
    const dLinear = deltaTime * this.params.lv;
    const direction = this.direction;
    const jointControls = this.jointControls;
    const robot = this.robot;
    var needUpdate = false;

    for( let key in jointControls ) {

      const joint = robot.joints[key];

      if( joint ) {
        const angle = joint.angle + jointControls[key] * dAngle;
        joint.setJointValue(angle);
      }

      needUpdate = true;
      
    }

    const jointValues = this.getJointValues();

    if( direction['up'] || direction['down'] 
      || direction['front'] || direction['back']
      || direction['left'] || direction['right'] ){

      const pose = getTCP(jointValues);
      
      if( direction['up'] ) {
        pose[2] += dLinear; // three.js 的Y 对应机械臂的 Z
      }
      if( direction['down'] ) {
        pose[2] -= dLinear; // z
      }

      if( direction['front'] ) {
        pose[1] += dLinear; 
      }
      if( direction['back'] ) {
        pose[1] -= dLinear; // z
      }

      if( direction['left'] ) {
        pose[0] += dLinear; 
      }
      if( direction['right'] ) {
        pose[0] -= dLinear; // z
      }

      let targetValues = findNearest(jointValues, getInverse(pose, 'UR5'));

      if( targetValues ) {
        console.log(">>D0:", pose, getTCP(targetValues), targetValues );

        this.moveToTarget(targetValues, dAngle);
        needUpdate = true;
      }
    }

    if( direction['zero'] ) {
      const zeroValues = this.state.robotList[this.state.selectIndex].robot.zeros;
      this.moveToTarget(zeroValues, dAngle);
      needUpdate = true;
    }

    if( direction['rl'] ) {
      
      needUpdate = true;
    }
    
    if( needUpdate ) {
      // let p = new THREE.Vector3();
      // let quat = new THREE.Quaternion();
      // robot.joints['ee_fixed_joint'].getWorldPosition(p);
      // robot.joints['ee_fixed_joint'].getWorldQuaternion(quat);
      // console.log(new Euler().setFromQuaternion(quat));
      // console.log(p);
      this.updateTCP();
    }
  }

  updateTCP() {
    const robot = this.robot;
    if( !robot ) return;
    const joints = this.state.robotList[this.state.selectIndex].robot.joints;
    const model = this.state.robotList[this.state.selectIndex].robot.model;
    const theta = [];
    for( var i = 0, il = joints.length; i < il; i++ ) {
      theta[i] = robot.joints[joints[i]].angle;
    }
    const tcp = getTCP(theta, model);

    this.setState({ 
      robot: robot,
      TCP: tcp 
    });
  }

  animate() {
    this.animation = window.requestAnimationFrame( this.animate.bind(this) );

    const renderer = this.renderer;
    const camera = this.camera;
    const scene = this.scene;
    const control = this.control;
    const stats = this.stats;

    if( renderer ){
      renderer.render( scene, camera );
    }

    if( control ){
      control.update();
    }

    if( stats ){
      stats.update();
    }

    const deltaTime = this.clock.getDelta();

    this.updateRobot( deltaTime );

  }

  loadRobotIndex( url ) {
    fetch(url).then( response=> { return response.json(); } )
    .then(json=>{
      const robotList = [];
      for( var key in json ) {
        robotList.push( {
          key:key,
          robot:json[key]
        });
      }

      this.setState({
        robotList: robotList,
        selectIndex: -1,
      })

    })
  }

  loadRobot( robot ) {
    // console.log( 'load robot:', robot );
    var urdf = robot.urdf;
    urdf = urdf.replace(".", `${this.baseUrl}/data`);
    const packages = robot.packages;

    for( var key in packages ) {
      packages[key] = packages[key].replace(".", `${this.baseUrl}/data`);
    }

    const manager = new LoadingManager();
    const loader = new URDFLoader( manager );

    loader.packages = packages;
    loader.load(
      urdf,
      robot=>{
        console.log( robot );
        // Z -> Y
        robot.rotateX(-Math.PI/2);
        this.initGraphics();
        this.scene.add( robot );

        // link/joint gui
        this.addRobotGui( robot );

        this.robot = robot;

        this.updateTCP();

      }
    )
  }

  addRobotGui( robot ){
    if( !robot ) return;
    const gui = this.gui;
    const params = this.params;
    
    // links
    const links = gui.addFolder('Links');
    for( let key in robot.links ){
      const link = robot.links[key];
      params[key] = true;
      links.add(params, key).onChange(val=>{
        link.visible = val;
      })
    }

    // joints
    const joints = gui.addFolder('Joints');
    for( let key in robot.joints ){
      const joint = robot.joints[key];
      if( joint._jointType === 'fixed' ) continue;
      params[key] = robot.joints[key].angle;
      joints.add(params, key).onChange(val=>{
        joint.setJointValue(val * Math.PI / 180);
      })
    }

  }

  addJointControls() {

    const robot = this.state.robot;
    if( !robot ) return;

    const joints = [];

    for( var key in robot.joints ){
      const joint = robot.joints[key];
      if( joint._jointType === 'fixed' ) continue;
      joints.push(key);
    }

    return (
      <div className='panel-sider'>
      <div className='flex-table'>
        {
        joints.map((value, index)=>
          <div key={index} className='row'>
            <button id={value} className='col rect-button'
              onMouseDown={this.handleJointValueDecrease} 
              onTouchStart={this.handleJointValueDecrease}
            >-</button>
            <label className='col'>{value}</label>
            <button id={value} className='col rect-button'
              onMouseDown={this.handleJointValueIncrease} 
              onTouchStart={this.handleJointValueIncrease}
            >+</button>
            <label className='col'>{(robot.joints[value].angle * 180 / Math.PI).toFixed(2)}</label>
          </div>)
        }
      </div>
      </div>
    )
  }

  addPositionControls() {
    const tcp = this.state.TCP;

    return(
      <div className='panel-lb'>
        <div className='flex-table'>
        <div className='row'>
          <div className='col circle-button' id='front' 
            onMouseDown={this.handleMouseDown}
            onTouchStart={this.handleMouseDown}
          >前</div>
          <div className='col circle-button' id='up' 
            onMouseDown={this.handleMouseDown}
            onTouchStart={this.handleMouseDown}
          >上</div>
          <div className='col circle-button' id='back' 
            onMouseDown={this.handleMouseDown}
            onTouchStart={this.handleMouseDown}
          >后</div>
        </div>
        <div className='row'>
          <div className='col circle-button' id='left' 
            onMouseDown={this.handleMouseDown}
            onTouchStart={this.handleMouseDown}
          >左</div>
          <div className='col circle-button' id='zero'
            onMouseDown={this.handleMouseDown}
            onTouchStart={this.handleMouseDown}
          >零</div>
          <div className='col circle-button' id='right'
            onMouseDown={this.handleMouseDown}
            onTouchStart={this.handleMouseDown}
          >右</div>
        </div>
        <div className='row'>
          <div className='col'></div>
          <div className='col circle-button' id='down' 
            onMouseDown={this.handleMouseDown}
            onTouchStart={this.handleMouseDown}
          >下</div>
          <div className='col'></div>
        </div>
        </div>
        <div className='flex-table'>
          <div className='row'>
            <div className='col calign-right'>X:</div>
            <span className='col'>{(tcp[0]*1000).toFixed(2)}</span>
            <div className='col'>mm</div>
          </div>
          <div className='row'>
            <div className='col calign-right'>Y:</div>
            <span className='col'>{(tcp[2]*1000).toFixed(2)}</span>
            <div className='col'>mm</div>
          </div>
          <div className='row'>
            <div className='col calign-right'>Z:</div>
            <span className='col'>{(tcp[1]*1000).toFixed(2)}</span>
            <div className='col'>mm</div>
          </div>
          <div className='row'>
            <div className='col calign-right'>RX:</div>
            <span className='col'>{tcp[3].toFixed(3)}</span>
            <div className='col'>rad</div>
          </div>
          <div className='row'>
            <div className='col calign-right'>RY:</div>
            <span className='col'>{tcp[5].toFixed(3)}</span>
            <div className='col'>rad</div>
          </div>
          <div className='row'>
            <div className='col calign-right'>RZ:</div>
            <span className='col'>{tcp[4].toFixed(3)}</span>
            <div className='col'>rad</div>
          </div>
        </div>
      </div>
    )
  }

  addServerPanel() {
    return(
      <div className='panel-lt'>
        <div className='flex-table'>
          <div className='row'>
            <div className='col txt-bold'>地址:</div>
            <div className='col'><input id='ip' type='text' value={this.state.serverIp} onChange={this.handleServerChanged}/></div>
          </div>
          <div className='row'>
            <div className='col txt-bold'>端口:</div>
            <div className='col'><input id='port' type='text' value={this.state.serverPort} onChange={this.handleServerChanged}/></div>
          </div>
          <div className='row'>
            <div className='col txt-bold'>同步:</div>
            <div className='col'><input id='sync' type='checkbox' value={this.state.serverSync} onChange={this.handleServerChanged}/></div>
          </div>
          <div className='row'>
            <div className='col'></div>
            <div className='col'>
              <div className='flex-row'>
              <button className='align-left' onClick={this.connect}>连接</button>
              <button className='align-right' onClick={this.disconnect}>断开</button>
              </div>
            </div>
          </div>
        </div>
      </div>
    )
  }

  debug() {
    const theta = [
      Math.random()*360-180,
      Math.random()*360-180,
      Math.random()*360-180,
      Math.random()*360-180,
      Math.random()*360-180,
      Math.random()*360-180,
    ]

    console.log('theta:', theta );
    const t = getDTCP(theta);
    console.log('TCP:', t);
    // const pos = [0, 300, 800, 0, 0, 0];
    const thetas = getInverse(t);
    // console.log(thetas);
    const findTheta = findNearest( theta, thetas );
    console.log(getTCP(findTheta));
  }

  showIndex() {
    return(
      <div className='main'>
        <h1 className='valign-middle'>选择机械臂开始</h1>
        <button onClick={this.debug.bind(this)}>测试用按钮</button>
        <div className='flex-panel'>
          {this.state.robotList.map(( value, index )=>{
            return (
              <div className='index' key={index} id={index} onClick={this.handleIndexChanged}>
                <div className='button'>{value.key}</div>
              </div>
            )
          })}
        </div>
      </div>
    )
  }

  showSimulator() {
    return(
      <div className='main'>
        <div className='container' id='container' ref={this.refContainer}>加载机械臂...</div>
        {this.state.showServerPanel ? this.addServerPanel() : ""}
        {this.state.showPositionPanel ? this.addPositionControls() : ""}
        {this.state.showJointPanel ? this.addJointControls() : ""}
      </div>
    )
  }

  render(){

    const selected = parseInt(this.state.selectIndex);
    const robotList = this.state.robotList;
    
    if( selected >= 0 && selected < robotList.length ) {
      return this.showSimulator();
    }
    else {
      return this.showIndex();
    }
  }

}